%roser2.m
%takes gmax,fov,nres,gam,
%computes k-trajectory kss for simulation tests
%D Twieg 8/06
%modified by mbolding@uab.edu 9/07
%                         ____              
%  _ __ ___  ___  ___ _ _|___ \   _ __ ___  
% | '__/ _ \/ __|/ _ \ '__|__) | | '_ ` _ \ 
% | | | (_) \__ \  __/ |  / __/ _| | | | | |
% |_|  \___/|___/\___|_| |_____(_)_| |_| |_|
%                                           
% play around with values in the lead-in -out section to tweak the initial
% and final sections of the trajectory. The gradients should start and end
% near zero as should the k-trajectory

% options
warning off all
trapz=0;
circum=1;  % circumscribed (=1) or not (not =1).

% hardware
MAX_DAC_VAL=32767.0;% DAC max integer, corresponds to gro, gpe
GDELT=1e-05;        % gradient waveform DAC speed


% constants
gam=4258.;
twopi=2*pi;
sq2=sqrt(2); 

%tweakers
gmax=2.29; % maximum read gradient to be used.
pre_emph=1; % pre emphisis to compensate for high freq attenuation
L = [ 0.78   1.3  -1.03 0.4   0.51  0.93  -1.37  -0.22]; %lead in out tweakers
%      in,real     in,imag    out,real   out,imag

% other parameters of desired trajectory
fov=12.8;  % field of view in cm
nres=64;   % resolution
Ta=0.065;   % gradient duration in seconds without lead-in and -out
kf=nres/(2*fov); 

delt=1./(gam*gmax*fov); % ADC sample rate
Na=floor(Ta/delt); % number of acqusitions
tim=(0:Na-1)*delt; % vector of time points

if circum == 1     % circumscribed or not
    kra=sqrt(1.1284)*kf;
    Ne=2*round(sqrt(2)*nres/2);
else
    kra=kf;
    Ne=nres;
end

om1=gam*gmax/kra;
om2=(41/nres)*om1;

% generate vector of gradient points
gs=kra*(pre_emph*(om1+om2)*(cos((om1+om2)*tim+pi/2)+1i*sin((om1+om2)*tim+pi/2))...
  + (om2-om1)*(cos((om2-om1)*tim+pi/2)+1i*sin((om2-om1)*tim+pi/2)))/(2*gam);

% generate lead-in and lead-out
% play with the internal points of the trapezoids or splines to get the
% lead-in and -out correct.
num_pts = 100;
pts = 1:num_pts;
if trapz==1
    gs_li = [linspace(0,0,num_pts/3) linspace(0,0,num_pts/3) linspace(0,gs(1),num_pts/3)];     % lead-in  part
    gs_lo = [linspace(gs(end),0,num_pts/3) linspace(0,0,num_pts/3) linspace(0,0,num_pts/3)];     % lead-out  part
    gs = [gs_li gs gs_lo]; % put the lead-in -out and gradient wave together
else
    ctl_pts_x = [0 num_pts/3  2*num_pts/3 num_pts];
    %  
    %                                          |      |  adjust with these
    %                                          |      |  columns
    %                                          V      V
    gs_lir = spline(ctl_pts_x,[ 0             L(1)  L(2)   real(gs(1))    ],pts);     % lead-in real part
    gs_lii = spline(ctl_pts_x,[ 0             L(3)  L(4)   imag(gs(1))    ],pts);     % lead-in imag part
    gs_lor = spline(ctl_pts_x,[ real(gs(end)) L(5)  L(6)     0            ],pts);     % lead-out real part
    gs_loi = spline(ctl_pts_x,[ imag(gs(end)) L(7)  L(8)     0            ],pts);     % lead-out imag part
    gs_li = (gs_lir+i*gs_lii);
    gs_lo = (gs_lor+i*gs_loi);
    gs = [gs_li gs gs_lo]; % put the lead-in -out and gradient wave together
end
gs_max=max(abs(gs));

% predict k trajectory vector by integrating gs
ks2=kra+gam*delt*cumsum(gs);
ks2=ks2-ks2(1);  % has to start at zero

% compute resampled and scaled output vectors for UnityInova hardware
Q=10000; % we resample at P/Q rate, P, Q must be pos. int.
P=round(Q*delt/GDELT); % downsample because grad DAC is slower than ADC
dac = gs*MAX_DAC_VAL/gs_max; %use computed max dac value  not gmax (problem?)
dac_r=resample(real(dac),P,Q); 
dac_i=resample(imag(dac),P,Q); 


% show that puppy
len_d=100;
len_d_k=1000; % number of points to display on lead-in and -out
figure(1)
subplot(2,3,1);
plot(dac_i,1:size(dac_i,2),dac_r,1:size(dac_i,2)); grid on;
title(['dac gs_m_a_x=' num2str(gs_max)]);

subplot(2,3,2);
plot(1:len_d,dac_r(1:len_d),'o',1:len_d,dac_i(1:len_d),'o',...
    (1:len_d*Q/P)*P/Q,real(dac(1:len_d*Q/P)),(1:len_d*Q/P)*P/Q,imag(dac(1:len_d*Q/P))); grid on;
title('dac start');

subplot(2,3,3);
plot(1:len_d,dac_r(end-len_d+1:end),'+',1:len_d,dac_i(end-len_d+1:end),'+'); grid on;
title('dac end');

subplot(2,3,4);
plot(ks2); grid on;
title('ks')

subplot(2,3,5);
plot(ks2(1:len_d_k));grid on;
hold on
plot(ks2(1:2),'+r')
hold off
title('ks start')

subplot(2,3,6);
plot(ks2(end-len_d_k:end));grid on;
hold on
plot(ks2(end-2:end),'+r');grid on;
hold off
title('ks end')

figure(2)

subplot(1,2,1);
plot(1:len_d_k,real(ks2(1:len_d_k)),'+',1:len_d_k,imag(ks2(1:len_d_k)),'+');grid on;
title('ks start')

subplot(1,2,2);
plot(1:len_d_k,real(ks2(end-len_d_k+1:end)),'+',1:len_d_k,imag(ks2(end-len_d_k+1:end)),'+');grid on;
title('ks end')



% %roser2a.m
% %takes gmax,fov,nres,gam,
% %computes k-trajectory kss for simulation tests
% %(i.e., trajectory does not include wind-up and wind-down sections required in actual
% %implementation)
% %D Twieg 8/06
% %constants
% gam=4258.;twopi=2*pi;sq2=sqrt(2);
% 
% %parameters of desired trajectory
% gmax=4.5;%maximum read gradient to be used.
% fov=12.8;%field of view
% nres=64;%resolution
% circum=1;%circumscribed (1) or not (not 1).
% Ta=.065;%duration in seconds
% 
% kf=nres/(2*fov);
% delt=1./(gam*gmax*fov);
% Na=floor(Ta/delt);
% tim=(0:Na-1)*delt;
% 
% if circum == 1
%     kra=sqrt(2)*kf;
%     Ne=2*round(sqrt(2)*nres/2);
% else
%     kra=kf;
%     Ne=nres;
% end
% 
% om1=gam*gmax/kra;
% om2=(41/nres)*om1;
% gs=kra*(1.05*(om1+om2)*(cos((om1+om2)*tim+pi/2)+1i*sin((om1+om2)*tim+pi/2))...
%     + (om2-om1)*(cos((om2-om1)*tim+pi/2)+1i*sin((om2-om1)*tim+pi/2)))/(2*gam);
% ks2=kra+gam*delt*cumsum(gs);


